package robots.correction;

import java.util.HashMap;

import robots.CorrectionMode;
import robots.analyser.DistanceToWall;

import maze.Direction;

public abstract class RobotCorrector {
	public int lowerCorrectionLimit = 16;
	public int upperCorrectionLimit=19;
	public int extraCorrection=0;
	public int robotAngleCorrection=2;
	public RobotCorrector(){
		
	}
	public void correctToWalls(HashMap<Direction, DistanceToWall> walls, Direction robotDirection) {		
//		Direction robotDirection = generateDirection();
		
		boolean checkedHorizontal = false;
		boolean checkedVertical = false;
		for(Direction dir:walls.keySet())
		{			
			if((dir == Direction.EAST || dir == Direction.WEST) && walls.get(dir).WallType == 1 && !checkedHorizontal)
			{
				getInMiddle(robotDirection, dir, walls.get(dir).Distance);
				checkedHorizontal = true;
			}
			if((dir == Direction.NORTH || dir == Direction.SOUTH) && walls.get(dir).WallType == 1 && !checkedVertical)
			{
				getInMiddle(robotDirection, dir, walls.get(dir).Distance);
				checkedVertical = true;
			}
		}		
	}
	protected abstract void travel(int distance);
	protected abstract void rotate(int degrees);
	public void getInMiddle(Direction ownDir, Direction wallDir, int distance)
	{
		int rotationToCorrect = ownDir.PositiveAngleToDirection(wallDir);
		if (distance < lowerCorrectionLimit) {	
					//(this.getSensorRotation() % 360 + 360) % 360;
			if (rotationToCorrect > 180)
				rotationToCorrect = rotationToCorrect - 360;
			this.rotate(rotationToCorrect);
			this.travel(distance - upperCorrectionLimit-extraCorrection);
			this.rotate(-rotationToCorrect);
		} else if (distance > upperCorrectionLimit) {
			//int rotationToCorrect = (this.getSensorRotation() % 360 + 360) % 360;
			if (rotationToCorrect > 180)
				rotationToCorrect = rotationToCorrect - 360;
			else if (rotationToCorrect == 180)
				rotationToCorrect = 0;
			this.rotate(-rotationToCorrect);
			this.travel(-distance + upperCorrectionLimit-extraCorrection);
			this.rotate(rotationToCorrect);
		}
	}

 	protected float getAngle(int begin, int end) {
		double v = Math.abs(begin - end);
		float angle = (float) Math.atan(v / 40); //Nog in radians	
		angle *= 180 / Math.PI;
		if (begin > end) {
			angle *= -1;
		}
		return angle; //In graden
	}
 	public void correctAngle(int begin, int end, int sign){

 		if (end > 23||begin>23)
			return;
		if (Math.abs(begin - end) > robotAngleCorrection) {
			int angle = Math.round(getAngle(begin, end));
			this.rotate(sign*angle);
		}
 	}
	public abstract void lineCorrect();
}
